#include "calibration/sensor_cali.h"
#include "kalman_filter/kalman.h"

#ifndef GRAVITY
#	define GRAVITY 9.8015f
#endif

static float accState[6] = {0.0f};
static float accP[36] = {
	10.0f,	0.0f,	0.0f,	0.0f,	0.0f,	0.0f,
	0.0f,	10.0f,	0.0f,	0.0f,	0.0f,	0.0f,
	0.0f,	0.0f,	10.0f,	0.0f,	0.0f,	0.0f,
	0.0f,	0.0f,	0.0f,	10.0f,	0.0f,	0.0f,
	0.0f,	0.0f,	0.0f,	0.0f,	10.0f,	0.0f,
	0.0f,	0.0f,	0.0f,	0.0f,	0.0f,	10.0f
};
static float accH[6] = {0.0f};
static float accR = 10.0f;

void acc_cali_measure(void *vfilter, float *estimate_output);
void acc_cali_computeH(void *vfilter);
void acc_cali_computeR(void *vfilter);

static struct kalman_filter acc_cali_kf = {
	.state_dim = 6,
	.measure_dim = 1,
	.x = accState,
	.P = accP,
	.A = NULL,
	.H = accH,
	.Q = NULL,
	.R = &accR,
	.process = NULL,
	.measurement = acc_cali_measure,
	.computeA = NULL,
	.computeQ = NULL,
	.computeH = acc_cali_computeH,
	.computeR = acc_cali_computeR
};

static struct acc_cali_monitor {
	float last_state[6];
	uint16_t converge_cnt;

	float prev_sel_acc[3];
	uint8_t too_close_cnt;

	uint8_t reset_filter;
	uint8_t status;
} acc_cali_monitor = {
	.status = CALI_DONE
};

uint8_t acc_calibration(int16_t *raw,
			struct ellipsoid_param *res,
			uint8_t do_cali)
{
	/* variables for convergency check */
	uint8_t i;
	float variance[6] = {0.0};

	float fAcc[3] = { raw[0], raw[1], raw[2] };
	/* for Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1 */
	float measure = 1.0;

	if (acc_cali_monitor.status != CALI_PROCESSING) {
		if (0 == do_cali) {
			return CALI_DONE;
		}
		acc_cali_monitor.status = CALI_PROCESSING;
		acc_cali_monitor.reset_filter = 1;
	}

	if (0 != acc_cali_monitor.reset_filter) {
		acc_cali_monitor.reset_filter = 0;
		/* reset acc calibration monitor */
		acc_cali_monitor.converge_cnt = 0;
		acc_cali_monitor.too_close_cnt = 0;
		acc_cali_monitor.last_state[0] = 0.0f;
		acc_cali_monitor.last_state[1] = 0.0f;
		acc_cali_monitor.last_state[2] = 0.0f;
		acc_cali_monitor.last_state[3] = 0.0f;
		acc_cali_monitor.last_state[4] = 0.0f;
		acc_cali_monitor.last_state[5] = 0.0f;
		/* reset filter */
		acc_cali_kf.x[0] = 1.0f;
		acc_cali_kf.x[1] = 1.0f;
		acc_cali_kf.x[2] = 1.0f;
		acc_cali_kf.x[3] = 1.0f;
		acc_cali_kf.x[4] = 1.0f;
		acc_cali_kf.x[5] = 1.0f;

		acc_cali_kf.P[0] = 10.0f;
		acc_cali_kf.P[7] = 10.0f;
		acc_cali_kf.P[14] = 10.0f;
		acc_cali_kf.P[21] = 10.0f;
		acc_cali_kf.P[28] = 10.0f;
		acc_cali_kf.P[35] = 10.0f;
	}

	if (vec3_norm(fAcc[0] - acc_cali_monitor.prev_sel_acc[0],
		fAcc[1] - acc_cali_monitor.prev_sel_acc[1],
		fAcc[2] - acc_cali_monitor.prev_sel_acc[2])
		/ vec3_norm(fAcc[0], fAcc[1], fAcc[2]) < 0.5) {
		if (acc_cali_monitor.too_close_cnt > 5)
			return CALI_PROCESSING;
		acc_cali_monitor.too_close_cnt++;
	} else {
		acc_cali_monitor.too_close_cnt = 0;
		acc_cali_monitor.prev_sel_acc[0] = fAcc[0];
		acc_cali_monitor.prev_sel_acc[1] = fAcc[1];
		acc_cali_monitor.prev_sel_acc[2] = fAcc[2];
	}

	acc_cali_kf.H[0] = fAcc[0] * fAcc[0];
	acc_cali_kf.H[1] = fAcc[1] * fAcc[1];
	acc_cali_kf.H[2] = fAcc[2] * fAcc[2];
	acc_cali_kf.H[3] = 2 * fAcc[0];
	acc_cali_kf.H[4] = 2 * fAcc[1];
	acc_cali_kf.H[5] = 2 * fAcc[2];

	kf_update(&acc_cali_kf, &measure);

	for (i = 0; i < 6; i++) {
		if (acc_cali_kf.x[i] < 1e-7 && acc_cali_kf.x[i] > -1e-7) {
			variance[i] = 1.0f;	/* avoid divide by zero */
		} else {
			variance[i] = (acc_cali_kf.x[i]
					- acc_cali_monitor.last_state[i])
					/ acc_cali_kf.x[i];
		}
		if (variance[i] < 0.0f) {
			variance[i] = -variance[i];
		}
		if (variance[i] > 0.05) { /* choosen by experience */
			acc_cali_monitor.converge_cnt = 0;
			acc_cali_monitor.last_state[0] = acc_cali_kf.x[0];
			acc_cali_monitor.last_state[1] = acc_cali_kf.x[1];
			acc_cali_monitor.last_state[2] = acc_cali_kf.x[2];
			acc_cali_monitor.last_state[3] = acc_cali_kf.x[3];
			acc_cali_monitor.last_state[4] = acc_cali_kf.x[4];
			acc_cali_monitor.last_state[5] = acc_cali_kf.x[5];
			return CALI_PROCESSING;
		}
	}
	/* acc calibration end */
	if (acc_cali_monitor.converge_cnt++ > 100) {
		state_to_param(acc_cali_kf.x, res);
		acc_cali_monitor.status = CALI_DONE;
		return CALI_DONE;
	}
	return CALI_PROCESSING;
}

void acc_compensation(int16_t *raw, struct ellipsoid_param *res)
{
	raw[0] = (int16_t)((raw[0] - res->offset[0])
			   / res->scale[0] * GRAVITY * 1000);
	raw[1] = (int16_t)((raw[1] - res->offset[1])
			   / res->scale[1] * GRAVITY * 1000);
	raw[2] = (int16_t)((raw[2] - res->offset[2])
			   / res->scale[2] * GRAVITY * 1000);
}

void acc_cali_measure(void *vfilter, float *estimateOut)
{
	struct kalman_filter *filter = (struct kalman_filter *)vfilter;
	*estimateOut = filter->x[0] * filter->H[0]
			+ filter->x[1] * filter->H[1]
			+ filter->x[2] * filter->H[2]
			+ filter->x[3] * filter->H[3]
			+ filter->x[4] * filter->H[4]
			+ filter->x[5] * filter->H[5];
}

void acc_cali_computeH(void *vfilter)
{
	return;
}

void acc_cali_computeR(void *vfilter)
{
	return;
}

